#include "ActionScouting.h"

ActionScouting::ActionScouting(double speed, double degreeStep) :
	ArAction("Scouting") {

	mySpeed = speed;
	myDegreeStep = degreeStep;
	left=false;
	right=false;
	angle=0;
	startAngle=0;
	sign=-1;
	angleLeft=0;
	angleRight=390;
}

void ActionScouting::setRobot(ArRobot *robot) {
	ArAction::setRobot(robot);
	mySonar = robot->findRangeDevice("sonar");
	if (robot == NULL) {
		ArLog::log(ArLog::Terse,
				"actionExample: ActionGo: Warning: I found no sonar, deactivating.");
		deactivate();
	}
}

ArActionDesired *ActionScouting::fire(ArActionDesired currentDesired) {


	double delta,distance=0;


	myDesired.reset();
	delta=myRobot->getTh();

	angle=angle+fabs(fabs(startAngle)-fabs(delta));


	if(angle>=angleLeft)
		left=true;
	if(angle>=angleRight)
		right=true;

	if(right== true && left== true){

			myDesired.setVel(mySpeed);
			myCurrentDistance= myRobot->getOdometerDistance();
			distance= myCurrentDistance-myStartDistance;
			myDesired.setDeltaHeading(0);

			if(distance>2000){
				right=false;
				left=false;
				angle=0;
				startAngle=myRobot->getTh();
			}

	} else {
			myStartDistance=myRobot->getOdometerDistance();
			myCurrentDistance= myRobot->getOdometerDistance();
	  }

	if(left== false && right == false){

		myDesired.setRotVel(sign*30);
	}


	if((left==true && right== false) || (right==true && left==false)){

		myDesired.setRotVel(-sign*30);
	}

	startAngle=delta;

	return &myDesired;
}

